///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef _BUOLA_MAT_OPTIMIZE_NLOPT_H_
#define _BUOLA_MAT_OPTIMIZE_NLOPT_H_

#include <buola/mat.h>
#include <gsl/gsl_multimin.h>
#include <nlopt.hpp>

namespace buola { namespace mat {

namespace detail {

    struct CNLOptRowConstraint
    {
        int mRow;
        void *mRealClosure;
    };
    
    template<typename tFunc>
    class CNLOptAdapter
    {
    public:
        CNLOptAdapter(tFunc &pFunc,size_t pR,size_t pC)
            :   mFunc(pFunc)
            ,   mR(pR)
            ,   mC(pC)
        {}
        
        static double Func(unsigned int pN,const double *pX,double *pDF,void *pClosure)
        {
            CNLOptAdapter *lAdapter=(CNLOptAdapter*)pClosure;

            return lAdapter->CallFunc(pX);
        }
        
        static double ConstraintFunc(unsigned int pN,const double *pX,double *pDF,void *pClosure)
        {
            CNLOptRowConstraint *lConstraint=(CNLOptRowConstraint*)pClosure;
            CNLOptAdapter *lAdapter=(CNLOptAdapter*)lConstraint->mRealClosure;

            if(pDF)
            {
//                pDF.assign(pDF.size(),0.0);
                for(int k=0;k<lAdapter->mC;k++)
                    pDF[lConstraint->mRow*lAdapter->mC+k]=1.0;
            }
            
            return lAdapter->CallConstraintFunc(pX,lConstraint->mRow);
            
        }
        
    private:
        double CallFunc(const double *pX)
        {
            mat::CMat_d lMat(mR,mC);
            
            for(int r=0;r<mR;r++)
                for(int c=0;c<mC;c++)
                    lMat(r,c)=pX[r*mC+c];
            
            return mFunc.F(lMat);
        }
        
        double CallConstraintFunc(const double *pX,int pRow)
        {
            mat::CMat_d lMat(mR,mC);
            
            for(int r=0;r<mR;r++)
                for(int c=0;c<mC;c++)
                    lMat(r,c)=pX[r*mC+c];
            
            return mFunc.RowConstraint(lMat,pRow);
        }
        
    private:
        tFunc &mFunc;
        size_t mR,mC;
    };

/*namespace detail*/ }

template<typename tFunc>
CMat_d minimize_nlopt(const CMat_d &pInitialGuess,tFunc &pFunc,double pLB,double pUB,
                      double pStepSize,double pEpsAbs)
{
    nlopt::opt lOpt(nlopt::LN_COBYLA,pInitialGuess.size());
    lOpt.set_maxtime(5);
    lOpt.set_initial_step(0.01);
    lOpt.set_xtol_rel(1e-4);
    lOpt.set_lower_bounds(pLB);
    lOpt.set_upper_bounds(pUB);
    
    detail::CNLOptAdapter<tFunc> lAdapter(pFunc,pInitialGuess.Rows(),pInitialGuess.Cols());
    
    lOpt.set_min_objective(detail::CNLOptAdapter<tFunc>::Func,&lAdapter);
    
    detail::CNLOptRowConstraint lRowConstraints[pInitialGuess.Rows()];
    
    for(int t=0;t<pInitialGuess.Rows();t++)
    {
        lRowConstraints[t]={t,&lAdapter};
        lOpt.add_equality_constraint(detail::CNLOptAdapter<tFunc>::ConstraintFunc,&lRowConstraints[t],0.2);
    }
    
    std::vector<double> lX(pInitialGuess.size());

    for(int r=0;r<pInitialGuess.Rows();r++)
        for(int c=0;c<pInitialGuess.Cols();c++)
            lX[r*pInitialGuess.Cols()+c]=pInitialGuess(r,c);

    double lMinF;
    nlopt::result lStatus=lOpt.optimize(lX,lMinF);
    
    msg_info() << "f:" << lMinF << "\n";
    msg_info() << "result:" << (int)lStatus << "\n";
    
    mat::CMat_d lResult(pInitialGuess.Rows(),pInitialGuess.Cols());

    for(int r=0;r<pInitialGuess.Rows();r++)
        for(int c=0;c<pInitialGuess.Cols();c++)
            lResult(r,c)=lX[r*pInitialGuess.Cols()+c];
    
    return lResult;
}
    
/*namespace mat*/ } /*namespace buola*/ }

#endif
